#include "rclcpp/rclcpp.hpp"
#include "control_base/WMJRobotControl.h"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float64.hpp"
#include <thread>
#include "control/WMJRobotControl.h"
#include "port/CanPort.hpp"
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;
namespace wmj
{
    /**
     *@brief: 连发子弹
     *
     *@param:base_interfaces::msg::Shooter int msg.bulletnum发射数 float 子弹射速 float 拨弹轮转速
     * 子弹射速设置目前无效，可以在control_base/src下的源文件中改
     * 射击子弹数量 0为不射击(开摩擦轮)，n为射击n发(n>0)，-1为持续射击（此时持续发射，给0才会停）-2为解释能
     */
    void Control::shootSomeCallback(const base_interfaces::msg::Shooter::SharedPtr msg, GIMBAL_ORIENTATION orientation)
    {
        int bulletnum = msg->bulletnum;
        // if (m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::AIMER || m_gimbalControlPermissions == GIMBAL_CONTROL_PERMISSION::DEFAULT)
        //     return;
        if (bulletnum == -2)
        {
            RobotControl->StopShoot(orientation);
        }
        if (bulletnum == -1)
        {
            RobotControl->KeepShoot(orientation);
        }
        else if (bulletnum == 0)
        {
            RobotControl->ShootSome(0, orientation);
        }
        else if (bulletnum > 0)
        {
            RobotControl->ShootSome(bulletnum, orientation);
        }
    }
    /**
     *@brief: 停止发射，包括摩擦轮 ,base_interfaces::msg::Shooter msg; bool msg.stop1停,0不处理
     **/
    void Control::stopShootCallback(const base_interfaces::msg::Shooter::SharedPtr msg)
    {
        auto stop = msg->stop;
        if (stop == 1)
        {
            RobotControl->StopShoot(LEFT);
            RobotControl->StopShoot(RIGHT);
        }
    }
    /**
     *@brief: 开弹仓 base_interfaces::msg::Shooter msg; bool msg.box;1开0关
     */
    void Control::openBoxCallback(const base_interfaces::msg::Shooter::SharedPtr msg)
    {
        auto box = msg->box;
        if (box == 1)
        {
            RobotControl->openBox();
        }
    }
    /**
     *@brief: 是否拉高射速
     *
     *@return:base_interfaces::msg::Shooter msg, bool msg.shoot_mode是否拉高
     *1为高射速模式，2为低射速模式（从电控获取状态）
     *
     */
    void Control::highShootSpeedCallback()
    {
        bool shoot_mode = RobotControl->HighShootSpeed();
        auto msg = base_interfaces::msg::Shooter();
        msg.shoot_mode = shoot_mode;
        Control::m_highShootSpeedPublisher->publish(msg);
    }
    /**
     * @brief 获取连发剩余子弹数
     *
     * @return int msg.getbulletnumber
     */
    void Control::getBulletNumberCallback()
    {
        int getbulletnumber = RobotControl->GetBulletNumber();
        auto msg = base_interfaces::msg::Shooter();
        msg.getbulletnumber = getbulletnumber;
        Control::m_getBulletNumberPublisher->publish(msg);
    }

    //! !!!!!!!!!!!!!BtAimer现在用来控制发射权限，当bullet_rate=1时，给发射权限
    /**
     * @brief
     *
     * @param msg
     */
    void Control::shootControlCallback(const base_interfaces::msg::BtAimer::SharedPtr msg)
    {
        // if (msg->bullet >= 0)
        // {
        //     m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::DEFAULT;
        // }
        // RCLCPP_DEBUG(this->get_logger(), "bullet rate: %d", msg->bullet);
    }

}
